clear
close all
clc

%% 
origin_lon = 105.16388;
origin_lat = 38.463945;

%%
figure_num = 1;

%% 
num_uav = 10;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器任务分配航迹规划\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器任务分配航迹规划')

%% 
num_uav = 10;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器非平衡目标任务分配航迹规划\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器非平衡目标任务分配航迹规划')

%% 
num_uav = 10;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器重要程度差异型目标任务分配航迹规划\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器重要程度差异型目标任务分配航迹规划')

%% 
num_uav = 3;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器任务分配航迹规划-有禁飞区\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
forbid_area = read_forbid_area_data("数据\\20230325-禁飞区-3弹任务规划实验.obsd");

figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
forbid_area_visualize(forbid_area, origin_lon, origin_lat)

axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器任务分配航迹规划-有禁飞区')

%% 
line = read_flight_line_data("输出结果\\单飞行器航迹规划.fld");
trajectory = cbpath_to_trajectory(line, origin_lon, origin_lat);
forbid_area = read_forbid_area_data("数据\\20230325-禁飞区.obsd");

figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
plot(trajectory.X, trajectory.Y, 'r-', 'LineWidth', 1)
forbid_area_visualize(forbid_area, origin_lon, origin_lat)
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('单飞行器航迹规划')

%% 
num_uav = 5;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器编队航迹规划\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
forbid_area = read_forbid_area_data("数据\\20230325-禁飞区.obsd");

figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
forbid_area_visualize(forbid_area, origin_lon, origin_lat)
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器编队航迹规划')

%% 
num_uav = 5;
lines = cell(num_uav, 1);
trajectories = cell(num_uav, 1);
for i = 1 : num_uav
    lines{i, 1} = read_flight_line_data("输出结果\\多飞行器同时到达航迹规划-无禁飞区\\飞行器" + num2str(i) +".fld");
    trajectories{i, 1} = cbpath_to_trajectory(lines{i, 1}, origin_lon, origin_lat);
end
figure(figure_num); figure_num = figure_num + 1;
grid on
hold on
for i = 1 : num_uav
    plot(trajectories{i, 1}.X, trajectories{i, 1}.Y, '-', 'LineWidth', 1)
end
axis equal
xlabel('X(m)')
ylabel('Y(m)')
title('多飞行器同时到达航迹规划-无禁飞区')

%% 
function line = read_flight_line_data(file)
    fileData = importdata(file);
    fileData = fileData.data;
    line.Lon = fileData(:, 1);
    line.Lat = fileData(:, 2);
    line.Height = fileData(:, 3);
    line.Length = fileData(:, 5);
    line.Curvature = fileData(:, 6);
end

%% 
function area = read_forbid_area_data(file)
    fileData = importdata(file);
    fileData = fileData.data;
    area.AreaID = fileData(:, 1);
    area.PointType = fileData(:, 2);
    area.Lon = fileData(:, 3);
    area.Lat = fileData(:, 4);
    area.Height = fileData(:, 5);
    area.Radius = fileData(:, 6);
    area.StartAngle = fileData(:, 7);
    area.FinishAngle = fileData(:, 8);
end

%% 
function trajectory = polyline_to_trajectory(line, origin_lon, origin_lat)
    X = zeros(length(line.Lon), 1);
    Y = zeros(length(line.Lon), 1);
    for i = 1:length(line.Lon)
        [X(i), Y(i)] = GeoToCar(origin_lon, origin_lat, line.Lon(i), line.Lat(i));
    end
    trajectory.X = X;
    trajectory.Y = Y;
end

%% 
function trajectory = cbpath_to_trajectory(line, origin_lon, origin_lat)
    waypoints.X = zeros(length(line.Lon), 1);
    waypoints.Y = zeros(length(line.Lon), 1);
    waypoints.Z = line.Height;
    for i = 1:length(line.Lon)
        [waypoints.X(i), waypoints.Y(i)] = geo_to_car(origin_lon, origin_lat, line.Lon(i), line.Lat(i));
    end
    X = waypoints.X(1);
    Y = waypoints.Y(1);
    
    for i = 2:length(waypoints.X)
        x0 = waypoints.X(i - 1);
        y0 = waypoints.Y(i - 1);
        x1 = waypoints.X(i);
        y1 = waypoints.Y(i);
        if line.Curvature(i) == 0
            X = [X; x1];
            Y = [Y; y1];
        else
            radius = abs(1.0 / line.Curvature(i));
            chord_length = norm([x1 - x0, y1 - y0]);
            chord_angle = atan2(y1 - y0, x1 - x0);
            turning_angle = 2.0 * asin(chord_length / 2.0 / radius);
            start_angle = chord_angle + sign(line.Curvature(i)) * (pi/2.0 - turning_angle / 2.0);
            center_x = x0 + radius * cos(start_angle);
            center_y = y0 + radius * sin(start_angle);
            turning_start_angle = start_angle - pi;
            turning_finish_angle = turning_start_angle + turning_angle * sign(line.Curvature(i));
            angle = linspace(turning_start_angle, turning_finish_angle, 100);
            arcX = center_x + radius * [cos(angle)]';
            arcY = center_y + radius * [sin(angle)]';
            X = [X; arcX];
            Y = [Y; arcY];
        end
    
    end
    trajectory.X = X;
    trajectory.Y = Y;
end

%% 经纬度转北东地坐标系函数
function [x, y] = geo_to_car(lon0, lat0, lon1, lat1)
    temp = pi * 6378245.0 / 180.0;
    x = (lat1 - lat0) * temp;
    y = (lon1 - lon0) * temp * cos(lat1*pi/180.0);
end

%%
function [] = forbid_area_visualize(area, origin_lon, origin_lat)
    for i = 1 : length(area.AreaID)
        if area.PointType(i) == 2
            [center_x, center_y] = geo_to_car(origin_lon, origin_lat, area.Lon(i), area.Lat(i));
            start_angle = deg2rad(area.StartAngle(i));
            finish_angle = deg2rad(area.FinishAngle(i));
            radius = area.Radius(i);
            while finish_angle < start_angle
                finish_angle = finish_angle + 2*pi;
            end
            angle = linspace(start_angle, finish_angle, 10);
            arcX = center_x + radius * [cos(angle)]';
            arcY = center_y + radius * [sin(angle)]';
            X = [center_x; arcX; center_x];
            Y = [center_y; arcY; center_y];
            plot(X, Y, 'k', 'LineWidth', 2)
        end
    end
    polygon_area_plot_states = zeros(10, 1);
    for i = 1 : length(area.AreaID)
        current_area_id = area.AreaID(i);
        if polygon_area_plot_states(current_area_id) == 1
            continue;
        end
        point_indices = find(area.AreaID == current_area_id);
        Lon = area.Lon(point_indices);
        Lat = area.Lat(point_indices);
        X = zeros(length(Lon), 1);
        Y = zeros(length(Lon), 1);
        for i_point = 1 : length(Lon)
            [X(i_point), Y(i_point)] = geo_to_car(origin_lon, origin_lat, Lon(i_point), Lat(i_point));
        end
        X = [X; X(1)];
        Y = [Y; Y(1)];
        plot(X, Y, 'k', 'LineWidth', 2)
        polygon_area_plot_states(current_area_id) = 1;
    end
end

